function [vertAngle] = bodyaxisvertangle(CBL) %#codegen
%BODYAXISVERTANGLE 由方向余弦矩阵求体坐标系各轴与水平面的夹角
%
% Tips
% # 参考坐标系L为NED
%
% Input Arguments
% # CBL: 3*3*m矩阵
%
% Output Arguments
% # vertAngle: m*3向量，体坐标系X、Y、Z轴与水平面的夹角，轴在水平面上方为正，值域[-π/2, π/2]，单位rad

CBG = dcmlev2nav(CBL);
if size(CBG, 3) > 1
    vertAngle = atan(squeeze(CBG(3, :, :))./sqrt(squeeze(CBG(1, :, :)).^2 + squeeze(CBG(2, :, :)).^2))';
else
    vertAngle = atan(CBG(3, :)./sqrt(CBG(1, :).^2 + CBG(2, :).^2));
end
end